//
// Created by 周杰 on 2020/2/13.
//

#include "TestFlow.h"

const string v_path = "/Volumes/D/study/machinelearning/opencv/testpic/vtest.avi";


void TestFlow::test() {


    VideoCapture cap;
    Mat source, result, gray, lastGray;
    //上一桢和本桢的特征点，上一桢是给定的，本桢的是预测结果
    vector<Point2f> points[2], temp;
    //每一特征点检测状态
    vector<uchar> status;
    //每一特征点计算误差
    vector<float> err;

    cap.open(v_path);
    if (!cap.isOpened()) {
        cout << "open video fail." << endl;
        return;
    }
    for (;;) {
        cap >> source;
        if (source.empty()) {
            break;
        }
        cvtColor(source, gray, COLOR_BGR2GRAY);

        if (points[0].size() < 10) {
            goodFeaturesToTrack(gray, points[0], 500, 0.01, 20);
        }

        if (lastGray.empty()) {
            gray.copyTo(lastGray);
        }

        //计算光流
        calcOpticalFlowPyrLK(lastGray, gray, points[0], points[1], status, err);

        //删除误判点
        int counter = 0;

        for (int i = 0; i < points[1].size(); i++) {
            double dist = norm(points[1][i] - points[0][i]);
            //合理的特征追踪点,距离差不多为正确，太大不对，太小可能噪声
            if (status[i] && dist >= 2.0 && dist <= 20.0) {
                points[0][counter] = points[0][i];
                points[1][counter++] = points[1][i];
            }
        }

        points[0].resize(counter);
        points[1].resize(counter);

        //显示特征点和运动轨迹
        source.copyTo(result);
        for (int j = 0; j < points[1].size(); ++j) {
            line(result, points[0][j], points[1][j], Scalar(0, 0, 0xff));
            circle(result, points[1][j], 3, Scalar(0, 0xff, 0));
        }

        //将当前桢赋值成上一桢
        swap(points[0],points[1]);
        swap(lastGray,gray);

        imshow("source",source);
        imshow("result",result);

        char key = waitKey(100);
        if (key == 27) {
            break;
        }

    }

    waitKey();
    destroyAllWindows();
}